#include "robot.h"
// 构造函数，代码像这样写可以让一个类的成员赋值
robot::robot() : leftMotor(leftMotorPin_1, leftMotorPin_2), rightMotor(rightMotorPin_1, rightMotorPin_2) {}
// 运动学逆解，输入为二轮小车坐标系下的向前的速度v_x和角速度v_z，输出为两轮的速度
// 本函数并不是严谨的运算，只是存在这样的关系，加上机械结构简略，理论值和实际值必然存在很大偏差
void robot::ikine(float v_x, float v_z)
{
    float omega_left = v_x - v_z * WIDTH_OF_ROBOT / 2.0f;                         //计算出左轮的目标角速度
    float omega_right = v_x + v_z * WIDTH_OF_ROBOT / 2.0f;                        //计算出右轮的目标角速度
    Serial.printf("left speed:%.2f right speed:%.2f\n", omega_left, omega_right); //将两轮的目标角速度打印至串口
    leftMotor.motorRun(omega_left);                                               //输出至左电机
    rightMotor.motorRun(omega_right);                                             //输出至右电机
}
